#include <iostream>
#include <cstring>
#include <unistd.h>
#include <linux/can.h>
#include <sys/socket.h>
#include <net/if.h>
#include <arpa/inet.h>
#include <netpacket/packet.h>
#include <chrono>
#include <sys/ioctl.h>
#include <thread>
#include <mutex>
#include <vector>
#include <condition_variable>

struct Motor_State
{
    double motor_control = 0.0;
    double motor_velocity = 0.0;
    double motor_torque = 0.0;
};


class CanBus {
public:
    int Motor_1 = 4;
    
    std::mutex mutex_;  // 保护共享资源的互斥锁

    void send_can_data(const can_frame& frame) {
        // 在这里将数据发送到 CAN 总线（这只是一个模拟）
        if (write(sockfd, &frame, sizeof(struct can_frame)) != sizeof(struct can_frame)) {
            perror("Error sending CAN frame");
        }
    }
   
    // 接收CAN数据
    struct can_frame receive_can_data()
    {
        struct can_frame frame;
        read(sockfd, &frame, sizeof(struct can_frame));
        return frame;
    }

    CanBus();
    ~CanBus();

    void motor_init(int Node_Num);
    void feedback_init(int Node_Num);

private:
    
    // CAN配置
    int sockfd;
    struct ifreq ifr;
    struct sockaddr_can addr;


};

CanBus::CanBus()
{
    // 配置CAN通讯
    // 创建套接字
    sockfd = socket(PF_CAN, SOCK_RAW, CAN_RAW);
    if (sockfd < 0) {
        perror("Error creating socket");
    }

    // 设置 CAN 接口
    
    std::strcpy(ifr.ifr_name, "can0"); // 使用 can0 接口
    if (ioctl(sockfd, SIOCGIFINDEX, &ifr) < 0) {
        perror("Error getting interface index");
    }

    addr.can_family = AF_CAN;
    addr.can_ifindex = ifr.ifr_ifindex;

    // 绑定套接字
    if (bind(sockfd, (struct sockaddr*)&addr, sizeof(addr)) < 0) {
        perror("Error binding socket");
    }

    std::cout << "CAN配置完成" << std::endl;

    motor_init(Motor_1);
    feedback_init(Motor_1);
};

CanBus::~CanBus()
{
    close(sockfd);
}

void CanBus::motor_init(int Node_Num)
{
    struct can_frame frame_bit;
    frame_bit.can_id = 0x000;
    frame_bit.can_dlc = 2;
    frame_bit.data[0] = 0x01;
    frame_bit.data[1] = Node_Num;

    this->send_can_data(frame_bit);
    std::cout << "1" << std::endl;
    std::this_thread::sleep_for(std::chrono::microseconds(1000));

    frame_bit.can_dlc = 8;
    frame_bit.can_id = 0x600 + Node_Num;

    frame_bit.data[0] = 0x23;
    frame_bit.data[1] = 0x40;
    frame_bit.data[2] = 0x60;
    frame_bit.data[3] = 0x00;
    frame_bit.data[4] = 0x00;
    frame_bit.data[5] = 0x00;
    frame_bit.data[6] = 0x00;
    frame_bit.data[7] = 0x00;
    this->send_can_data(frame_bit);
    std::this_thread::sleep_for(std::chrono::microseconds(1000));

    // 设置加速度时间 100ms
    frame_bit.data[0] = 0x23;
    frame_bit.data[1] = 0x83;
    frame_bit.data[2] = 0x60;
    frame_bit.data[3] = 0x00;
    frame_bit.data[4] = 0x64;
    frame_bit.data[5] = 0x00;
    frame_bit.data[6] = 0x00;
    frame_bit.data[7] = 0x00;
    this->send_can_data(frame_bit);
    std::this_thread::sleep_for(std::chrono::microseconds(1000));

    // 设置减加速度时间 100ms
    frame_bit.data[0] = 0x23;
    frame_bit.data[1] = 0x84;
    frame_bit.data[2] = 0x60;
    frame_bit.data[3] = 0x00;
    frame_bit.data[4] = 0x64;
    frame_bit.data[5] = 0x00;
    frame_bit.data[6] = 0x00;
    frame_bit.data[7] = 0x00;
    this->send_can_data(frame_bit);
    std::this_thread::sleep_for(std::chrono::microseconds(1000));

    // 切换到速度模式
    frame_bit.data[0] = 0x2F;
    frame_bit.data[1] = 0x60;
    frame_bit.data[2] = 0x60;
    frame_bit.data[3] = 0x00;
    frame_bit.data[4] = 0x03;
    frame_bit.data[5] = 0x00;
    frame_bit.data[6] = 0x00;
    frame_bit.data[7] = 0x00;
    this->send_can_data(frame_bit);
    std::this_thread::sleep_for(std::chrono::microseconds(1000));

    // 切换驱动器状态机
    frame_bit.data[0] = 0x2B;
    frame_bit.data[1] = 0x40;
    frame_bit.data[2] = 0x60;
    frame_bit.data[3] = 0x00;
    frame_bit.data[4] = 0x06;
    frame_bit.data[5] = 0x00;
    frame_bit.data[6] = 0x00;
    frame_bit.data[7] = 0x00;
    this->send_can_data(frame_bit);
    std::this_thread::sleep_for(std::chrono::microseconds(1000));

    frame_bit.data[0] = 0x2B;
    frame_bit.data[1] = 0x40;
    frame_bit.data[2] = 0x60;
    frame_bit.data[3] = 0x00;
    frame_bit.data[4] = 0x07;
    frame_bit.data[5] = 0x00;
    frame_bit.data[6] = 0x00;
    frame_bit.data[7] = 0x00;
    this->send_can_data(frame_bit);
    std::this_thread::sleep_for(std::chrono::microseconds(1000));

    frame_bit.data[0] = 0x2B;
    frame_bit.data[1] = 0x40;
    frame_bit.data[2] = 0x60;
    frame_bit.data[3] = 0x00;
    frame_bit.data[4] = 0x0F;
    frame_bit.data[5] = 0x00;
    frame_bit.data[6] = 0x00;
    frame_bit.data[7] = 0x00;
    this->send_can_data(frame_bit);
    std::this_thread::sleep_for(std::chrono::microseconds(1000));
}

// (配置PDO映射)配置速度反馈和力矩反馈
void CanBus::feedback_init(int Node_Num)
{
    struct can_frame frame_bit;
    frame_bit.can_dlc = 8;
    frame_bit.can_id = 0x600 + Node_Num;

    // 清空映射
    frame_bit.data[0] = 0x2F;
    frame_bit.data[1] = 0x00;
    frame_bit.data[2] = 0x1A;
    frame_bit.data[3] = 0x00;
    frame_bit.data[4] = 0x00;
    frame_bit.data[5] = 0x00;
    frame_bit.data[6] = 0x00;
    frame_bit.data[7] = 0x00;
    this->send_can_data(frame_bit);
    std::this_thread::sleep_for(std::chrono::microseconds(1000));

    // 将606C 00映射到1A 00 01
    frame_bit.data[0] = 0x23;
    frame_bit.data[1] = 0x00;
    frame_bit.data[2] = 0x1A;
    frame_bit.data[3] = 0x01;
    frame_bit.data[4] = 0x20;
    frame_bit.data[5] = 0x00;
    frame_bit.data[6] = 0x6C;
    frame_bit.data[7] = 0x60;
    this->send_can_data(frame_bit);
    std::this_thread::sleep_for(std::chrono::microseconds(1000));

    // 将6077 00映射到1A 00 02
    frame_bit.data[0] = 0x23;
    frame_bit.data[1] = 0x00;
    frame_bit.data[2] = 0x1A;
    frame_bit.data[3] = 0x02;
    frame_bit.data[4] = 0x10;
    frame_bit.data[5] = 0x00;
    frame_bit.data[6] = 0x77;
    frame_bit.data[7] = 0x60;
    this->send_can_data(frame_bit);
    std::this_thread::sleep_for(std::chrono::microseconds(1000));

    // 设置TPDO0的COB-ID为184
    frame_bit.data[0] = 0x23;
    frame_bit.data[1] = 0x00;
    frame_bit.data[2] = 0x18;
    frame_bit.data[3] = 0x01;
    frame_bit.data[4] = 0x84;
    frame_bit.data[5] = 0x01;
    frame_bit.data[6] = 0x00;
    frame_bit.data[7] = 0x00;
    this->send_can_data(frame_bit);
    std::this_thread::sleep_for(std::chrono::microseconds(1000));

    // 设置TPDO0的传输方式为定时器触发
    frame_bit.data[0] = 0x2F;
    frame_bit.data[1] = 0x00;
    frame_bit.data[2] = 0x18;
    frame_bit.data[3] = 0x02;
    frame_bit.data[4] = 0xFF;
    frame_bit.data[5] = 0x00;
    frame_bit.data[6] = 0x00;
    frame_bit.data[7] = 0x00;
    this->send_can_data(frame_bit);
    std::this_thread::sleep_for(std::chrono::microseconds(1000));

    // 设置定时器周期为500ms
    frame_bit.data[0] = 0x2B;
    frame_bit.data[1] = 0x00;
    frame_bit.data[2] = 0x18;
    frame_bit.data[3] = 0x05;
    frame_bit.data[4] = 0x01;
    frame_bit.data[5] = 0x00;
    frame_bit.data[6] = 0x00;
    frame_bit.data[7] = 0x00;
    this->send_can_data(frame_bit);
    std::this_thread::sleep_for(std::chrono::microseconds(1000));

    // 启动TPDO0映射
    frame_bit.data[0] = 0x2F;
    frame_bit.data[1] = 0x00;
    frame_bit.data[2] = 0x1A;
    frame_bit.data[3] = 0x00;
    frame_bit.data[4] = 0x02;
    frame_bit.data[5] = 0x00;
    frame_bit.data[6] = 0x00;
    frame_bit.data[7] = 0x00;
    this->send_can_data(frame_bit);
    std::this_thread::sleep_for(std::chrono::microseconds(1000));

    // 保存TDPO映射到EEPROM
    frame_bit.data[0] = 0x2B;
    frame_bit.data[1] = 0x09;
    frame_bit.data[2] = 0x20;
    frame_bit.data[3] = 0x00;
    frame_bit.data[4] = 0x02;
    frame_bit.data[5] = 0x00;
    frame_bit.data[6] = 0x00;
    frame_bit.data[7] = 0x00;
    this->send_can_data(frame_bit);
    std::this_thread::sleep_for(std::chrono::microseconds(1000));
}

double Speed_Get(struct can_frame frame)
{
    int32_t temp_data = (frame.data[3]<< 24 )| (frame.data[2] << 16) | (frame.data[1] << 8) | frame.data[0];
    double speed = static_cast<double>(temp_data) * 0.1;
    return (speed);
}

double Torque_Get(struct can_frame frame)
{
    int16_t temp_data = (frame.data[5] << 8) | frame.data[4];
    double torque = static_cast<double>(temp_data) * 0.1;
    return (torque);
}

// 信号解析
void sigal_get(CanBus& bus)
{
    while (true)
    {
        struct can_frame frame;
        frame = bus.receive_can_data();
        // 解析位置代码
        if(frame.can_id == 0x180 + bus.Motor_1)
        {
            std::cout << "速度" << Speed_Get(frame) << std::endl;
            std::cout << "扭矩" << Torque_Get(frame) << std::endl;

        }
        // else if(frame.can_id == 0x580 + bus.Motor_1 && frame.data[0] == 0x43 && frame.data[1] == 0x6C && frame.data[2] == 0x60)
        // {
        //     std::cout << "速度" << Speed_Get(frame) << std::endl;
        // }
        // else if(frame.can_id == 0x580 + bus.Motor_1 && frame.data[0] == 0x43 && frame.data[1] == 0x77 && frame.data[2] == 0x60)
        // {
        //     std::cout << "电流" << Speed_Get(frame) << std::endl;
        // }

        std::this_thread::sleep_for(std::chrono::microseconds(100));
    }
}

struct can_frame Speed_Control_Frame(double speed, int Node_num)
{
    struct can_frame frame;
    int32_t can_all_data = static_cast<int32_t>(speed);

    frame.can_id = 0x600 + Node_num;
    frame.can_dlc = 8;
    frame.data[0] = 0x23;
    frame.data[1] = 0xFF;
    frame.data[2] = 0x60;
    frame.data[3] = 0x00;
    frame.data[4] = static_cast<uint8_t>(can_all_data & 0xFF);
    frame.data[5] = static_cast<uint8_t>((can_all_data >> 8) & 0xFF);
    frame.data[6] = static_cast<uint8_t>((can_all_data >> 16) & 0xFF);
    frame.data[7] = static_cast<uint8_t>((can_all_data >> 24) & 0xFF);

    return frame;

}


void zltech_motor_control(CanBus &canbus)
{
    while (1)
    {
        // 读取控制参数
        std::cout << "输入控制指令" << std::endl;
        double des_speed;
        std::cin >> des_speed;
        struct can_frame frame;
        frame = Speed_Control_Frame(des_speed, canbus.Motor_1);
        canbus.send_can_data(frame);
        std::this_thread::sleep_for(std::chrono::microseconds(1000));
    }
}

int main(int argc, char *argv[])
{
    CanBus canbus;
    std::thread motor1_control_thread(zltech_motor_control, std::ref(canbus));
    std::thread can_sigal_get(sigal_get, std::ref(canbus));

    motor1_control_thread.join();
    can_sigal_get.join();
    return 0;
}
